#-*- coding: utf-8 -*-
"""Abgabe U9A1 Pascal Trebert ComputerVision"""


import numpy as np

def get_cam_centre(P):
    """Berrechnet das Kamerazentrum einer 3x4 Kameramatrix"""
    x1 = np.linalg.det(P[:, 1:])
    x2 = - np.linalg.det(P[:, [0, 2, 3]])
    x3 = np.linalg.det(P[:, [0, 1, 3]])
    x4 = - np.linalg.det(P[:, :-1])
    c = x1/x4, x2/x4, x3/x4
    print 'Aufgabe 1.1'
    print '\nLinear berrechnet: c = %s' % str(c)

    P = np.array(P)
    u, s, v = np.linalg.svd(P)
    c = v[3]
    print '\nNumerisch mittels SVD berrechnet (homogen): ch = %s' % str(c/c[3])

    #print 'Numerisch mit (SVD) berrechnet: %s' % str(c /c[3])

def get_orientation(P):
    """Berrechnung Orientierugn R anhand manueller Cholesky-Zerlegung"""
    M = P[:,0:-1]
    MT = M.T
    KKT = np.dot(M, MT)

    a = KKT[0, 0]
    b = KKT[0, 1]
    c = KKT[2, 0]
    d = KKT[1, 1]
    e = KKT[2, 1]

    xv = e
    xu = c
    kv = np.sqrt(d- (e*e))
    s = (b - c*e) / kv
    ku = np.sqrt(a- ( ( (b - c * e) * (b - c * e)) / (d - e * e)) - c * c)
    k = np.array(((ku,s,xu),(0,kv,xv),(0,0,1)))
    print 'Aufgabe 1.2'
    print '\nKamera-Kalibriermatrix'
    print k
    r = np.dot(np.linalg.inv(k), M)
    print
    print 'Aufgabe 1.3'
    print "Orientierung/Rotation R"
    print r
    print
    print
    print 'Aufgabe 2'

    f = 6
    pixel = 0.005
    pixel = 1/pixel
    sensor = (640, 480)
    h = (310,250)
    C = np.array((100, 200, 300))

    R = np.array(((1, 0, 0), (0, 1, 0), (0, 0, 1)))
    I = np.array(((1, 0, 0, -C[0]), (0, 1, 0, -C[1]), (0, 0, 1, -C[2])))
    k1 = np.array(((pixel, 0, 0), (0, pixel, 0), (0, 0, 1)))
    k2 = np.array(((f, 0, h[0]), (0, f, h[1]), (0, 0, 1)))
    k = np.dot(k1, k2)

    kameramatrix = np.dot(np.dot(k,R), I)
    print "Kameramatrix"
    print kameramatrix


def main():
    pv = [[490, -390, -1500, 1300],
         [-590, 1400, -600, 1300],
         [-0.5 * np.sqrt(2), -0.3 * np.sqrt(2), -0.4 * np.sqrt(2), 5]
         ]

    P = np.matrix(pv)
    get_cam_centre(P)

    print 'Kamera-Orientierung'
    get_orientation(P)

if __name__ == '__main__':
    main()